#include <Servo.h>
#include <WiFi.h>

//舵机引脚
const int base_pin  = 6;
const int right_pin = 7;
const int left_pin  = 3;
const int claw_pin  = 10;
//舵机位置（度）
double base_pos  = 90;
double left_pos  = 45;
double right_pos = 90;
double claw_pos  = 90;
//舵机速度（度/秒）
double base_spd  = 90;
double left_spd  = 90;
double right_spd = 90;
double claw_spd  = 90;
//舵机加速度常量（K值）
double base_k  = 0.6;
double left_k  = 0.6;
double right_k = 0.6;
double claw_k  = 0;
//关节角度
double joint1 = 90;
double joint2 = 90;
double joint3 = 90;
double joint4 = 0;
//舵机角度补偿
double left_adj  = 20;
double right_adj = 0;
double base_adj  = 0;

Servo myservo;

//创建TCP服务器
WiFiServer server;
//客户端上位机
WiFiClient client;

void setup() {
  pinMode(12, OUTPUT);
  digitalWrite(12, LOW);
  Serial.begin(9600);
  //将WiFi工作模式设置为无线站模式
  WiFi.mode(WIFI_STA);
  //连接WiFi网络
  WiFi.begin("🤟", "12345678");
  //等待WiFi连接成功
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(100);
    Serial.print(".");
  }
  Serial.println("WiFi已连接，IP地址" + WiFi.localIP().toString());
  //联网成功后点亮LED
  digitalWrite(12, HIGH);
  //启动服务器，参数是服务端口
  server.begin(6666);
  Serial.println("网络服务已启动");
}

//弧度转角度
double rad2ang(double r)
{
  return r * 180 / PI;
}

//解析输入的命令
//命令格式：快速移动：G00 X坐标 Y坐标 Z坐标（单位厘米）
//        钳子开合：M55 [0|1]
void parse_cmd(const char* line)
{
    double x, y, z, d, a;
    
    //判断命令类型
    //控制钳子开合，参数1打开，参数0闭合
    if (strncmp(line, "M55", 3) == 0)
    {
      //解析命令，将输入的参数保存到变量中
      sscanf(line, "M55 %lf", &joint4);
      Serial.println("关节4:" + String(joint4));
      claw_pos = joint4 ? 105:85;
    }
    //快速移动
    if (strncmp(line, "G00", 3) == 0)
    {
      //解析命令，将输入的参数保存到变量中
      sscanf(line, "G00 X%lf Y%lf Z%lf", &x, &y, &z);
      //打印输入的参数
      Serial.print("X:" + String(x) + ",");
      Serial.print("Y:" + String(y) + ",");
      Serial.println("Z:" + String(z));
      //计算关节角度
      d = sqrt((x*x) + (y*y)) - 6.5;
      a = sqrt((d*d) + (z*z));
      double theta2 = atan(z/d);
      double theta1 = acos(a/16.0);
      joint2 = rad2ang(theta1 + theta2);
      joint3 = 2*(90 - rad2ang(theta1));
      if (x > 0)
      {
        joint1 = rad2ang(atan(y/x));
      }
      
      if (x == 0)
      {
        joint1 = 90;
      }
      
      if (x < 0)
      {
        joint1 = rad2ang(PI + atan(y/x));
      }
      
      //打印关节角度
      Serial.print("关节1:" + String(joint1) + ",");
      Serial.print("关节2:" + String(joint2) + ",");
      Serial.print("关节3:" + String(joint3) + ",");
      Serial.println("关节4:" + String(joint4));
      //计算舵机角度
      base_pos  = joint1;
      right_pos = 180 - joint2;
      left_pos  = joint2 + joint3 - 90;
      //打印舵机角度
      Serial.print("底:" + String(base_pos) + ",");
      Serial.print("左:" + String(left_pos) + ",");
      Serial.print("右:" + String(right_pos) + ",");
      Serial.println("钳:" + String(claw_pos));
    }
}

void loop() {
  if (Serial.available())
  {
    //从串口读取一行字符
    String line = Serial.readStringUntil('\n');
    //删除字符串前后的空白
    line.trim();
    parse_cmd(line.c_str());
  }

  //如果客户端没有连接
  if (!client.connected())
  {
    client = server.available();
    if (client)
    {
      Serial.println("客户端已连接，IP地址" + client.remoteIP().toString());
    }
  }
  else //客户端已连接
  {
    //接收到客户端发送的数据
    if (client.available())
    {
      String line = client.readStringUntil('\n');
      line.trim();
      parse_cmd(line.c_str());
    }
  }

  //转动舵机（每次循环走一步）
  myservo.write(base_pin, base_pos + base_adj, base_spd, base_k);
  myservo.write(left_pin, left_pos + left_adj, left_spd, left_k);
  myservo.write(right_pin, right_pos + right_adj, right_spd, right_k);
  myservo.write(claw_pin, claw_pos, claw_spd, claw_k);
}
